import roslibpy
import numpy as np
import spatialmath.base as smb
import math
from SAmp import SA

class rotorMeasuringModel:
    def __init__(self,typeName):
        self.nominalRotorAngle = [15.1,105.1,-164.9,-74.9]
        self.additionRotorAngle = [15.1+45.0,105.1+45.0,-164.9+45.0,-74.9+45.0]
        self.jigPoints=[[0.0,60.0,112.5],[0.0,60.0,-287.5],[0.0,-60.0,112.5],[0.0,-60.0,-287.5]]
        self.type = typeName
        self.rotorNum = 6

class nominalRotorConfig:
    def __init__(self,model):
        self.standardPanConfig = [0.0,0.0,0.0]
        self.zeroRotorAngle = {'val':-6.25,'range':[-0.1 , 0.1]}
        self.rotorWavingUpperLimit = [0.0] * model.rotorNum 
        self.currentRodLength = [0.0] * model.rotorNum 
        self.rodLengthRange = [[-1000.0,1000.0] for i in range(model.rotorNum)] 

        self.highTorqueDeltaAngle = {'val':{}}
        self.highTorqueDeltaAngle['val'][model.nominalRotorAngle[0]] = 15.0
        self.highTorqueDeltaAngle['val'][model.nominalRotorAngle[1]] = 15.0
        self.highTorqueDeltaAngle['val'][model.nominalRotorAngle[2]] = 15.0
        self.highTorqueDeltaAngle['val'][model.nominalRotorAngle[3]] = 15.0
    
    def updateRotorState(self,id,rodLength,limit):
        if(id>len(self.rotorWavingUpperLimit) or id<1):
            return False
        else:
            self.rotorWavingUpperLimit[id-1]=limit
            self.currentRodLength[id-1] = rodLength
        return True

    def updateRodLengthRange(self,id,rodrange):
        if(id>len(self.rotorWavingUpperLimit) or id<1):
            return False

        tempMin=max([self.rodLengthRange[id-1][0],rodrange[0]])
        tempMax = min([self.rodLengthRange[id-1][1],rodrange[1]])
        if(tempMin>tempMax):
            return False

        self.rodLengthRange[id-1][0] = tempMin
        self.rodLengthRange[id-1][1] = tempMax
        return True

class rotorModulingDesktop:
    def __init__(self, serverip):
        self.rosServer = None
        self.serverip = serverip
        self.SAClient = None
        self.rotorModel = rotorMeasuringModel('A903')

    def getValidRosServer(self):
        if self.rosServer is None:
            self.rosServer = roslibpy.Ros(host = self.serverip, port=9090)
            self.rosServer.run()
        return self.rosServer
    
    def close(self):
        if self.rosServer is not None:
            self.rosServer.terminate()
            self.rosServer = None

    def InsertTheoryJigPoints(self,SAInterface,name):
        x,y,z = SAInterface.get_point('A', name, 'p1')
        if(x==0.0 and y==0.0 and z==0.0):
            for i in range(len(self.rotorModel.jigPoints)):
                SAInterface.construct_point('A',name,'p'+str(i+1),self.rotorModel.jigPoints[i][0],self.rotorModel.jigPoints[i][1],self.rotorModel.jigPoints[i][2])

        return True
    
    def getPointsFromSA(self,SAInterface, groupid, poseNum):
        points = []
        for i in range(poseNum):
            pose = []
            for j in range(4):
                x,y,z = SAInterface.get_point('A', groupid, 'p'+str(i*4+j+1))
                pose.append([x,y,z])
            points.append(pose[:])
        return points

    def obtainHomoMatrix(self,SAInterface, groupid, points,thoeryName):
        SAInterface.delete_object('A', 'sim_pose'+str(groupid), 'Group')
        isDict = True if(isinstance(points[0],dict)) else False

        if(isDict):
            for i in range(len(points)):
                SAInterface.construct_point('A','sim_pose'+str(groupid),'p'+str(i+1),points[i]['x'],points[i]['y'],points[i]['z'])
        else:
            for i in range(len(points)):
                SAInterface.construct_point('A','sim_pose'+str(groupid),'p'+str(i+1),points[i][0],points[i][1],points[i][2])

        return SAInterface.best_fit('A', 'sim_pose'+str(groupid), thoeryName)

    def evaluatePanCommand(self,observation,panConfig,rotorId):
        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/evaluate_pan_command', 'rotors2/UpdateRotorParameters')
        
        panCommand = panConfig 
        rotorCommand = []
        for i in observation.keys():
            rotorCommand.append(i)
            rotorCommand.extend(observation[i][0])
            rotorCommand.extend(observation[i][1])
            rotorCommand.append(observation[i][3])

        # for i in observation.keys():
        request = roslibpy.ServiceRequest({'rotorId':rotorId,'panCommand':[0.0,0.0,0.0],'rotorCommand':{'data':rotorCommand}})
        # print('Calling service...')
        result = service.call(request)
        # print('Service response: {}'.format(result['isSucessful']))
        return result['parameters']

    def updateDTModel(self,observation,panConfig,rotorId):
        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/update_model_parameters', 'rotors2/UpdateRotorParameters')
        
        panCommand = panConfig 
        rotorCommand = []
        for i in observation.keys():
            rotorCommand.append(i)
            rotorCommand.extend(observation[i][0])
            rotorCommand.extend(observation[i][1])
            rotorCommand.append(observation[i][3])

        # for i in observation.keys():
        request = roslibpy.ServiceRequest({'rotorId':rotorId,'panCommand':panCommand,'rotorCommand':{'data':rotorCommand}})
        # print('Calling service...')
        result = service.call(request)
        # print('Service response: {}'.format(result['isSucessful']))
        return result['parameters']

    def modulingWithDTModel2(self,target,estPan,rotorId):
        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/module_rotor_delta_reference', 'rotors2/UpdateRotorParameters')
        
        panCommand = target.standardPanConfig 
        rotorCommand = []

        self.controlPanCommand(estPan[0],estPan[1],estPan[2])

        for i in self.rotorModel.nominalRotorAngle:
            rotorCommand.extend([i,target.rotorWavingUpperLimit[rotorId-1],target.highTorqueDeltaAngle['val'][i]]) 

        # for i in observation.keys():
        request = roslibpy.ServiceRequest({'rotorId':rotorId,'panCommand':panCommand,'rotorCommand':{'data':rotorCommand}})
        # print('Calling service...')
        result = service.call(request)
        # print('Service response: {}'.format(result['isSucessful']))
        return result['parameters']

    def modulingToRodRange(self,target,rotorId):
        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/module_rotor_parameters', 'rotors2/UpdateRotorParameters')
        
        panCommand = target.standardPanConfig 
        rotorCommand = []

        for i in self.rotorModel.nominalRotorAngle:
            rotorCommand.extend([i,target.rotorWavingUpperLimit[rotorId-1],target.zeroRotorAngle['val']+target.zeroRotorAngle['range'][0]]) 

        request = roslibpy.ServiceRequest({'rotorId':rotorId,'panCommand':panCommand,'rotorCommand':{'data':rotorCommand}})
        result1 = service.call(request)

        rotorCommand = []
        for i in self.rotorModel.nominalRotorAngle:
            rotorCommand.extend([i,target.rotorWavingUpperLimit[rotorId-1],target.zeroRotorAngle['val']+target.zeroRotorAngle['range'][1]]) 

        request = roslibpy.ServiceRequest({'rotorId':rotorId,'panCommand':panCommand,'rotorCommand':{'data':rotorCommand}})
        result2 = service.call(request)

        result=[max(result1['parameters']),min(result2['parameters'])]

        return result

    def modulingWithDTModel(self,target,panConfig,rotorId):
        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/module_rotor_parameters', 'rotors2/UpdateRotorParameters')
        
        panCommand = panConfig 
        rotorCommand = []

        for i in self.rotorModel.nominalRotorAngle:
            rotorCommand.extend([i,target.rotorWavingUpperLimit[rotorId-1],target.zeroRotorAngle['val']]) 

        # for i in observation.keys():
        request = roslibpy.ServiceRequest({'rotorId':rotorId,'panCommand':panCommand,'rotorCommand':{'data':rotorCommand}})
        # print('Calling service...')
        result = service.call(request)
        # print('Service response: {}'.format(result['isSucessful']))
        return result['parameters']

    def extractVerifyingObservations(self,SAInterface, measuredMatrix):
        result = SAInterface.get_circle_properties('A','rotorAxis')

        vectorz = np.array(result[1]) if result[1][2]>0 else -np.array(result[1])

        # vectorz=np.array([0.0,0.0,1.0])
        observ = {}
        for i in measuredMatrix.keys(): 
            observMatrix = np.linalg.inv(measuredMatrix[i])
            obsvCircleCenter = observMatrix@smb.transl(result[0][0],result[0][1],result[0][2]) 
            obsvDirection = observMatrix[0:3,0:3]@vectorz
            #measuredAngle = [math.acos(obsvDirection[1])-math.pi/2,math.acos(obsvDirection[0])-math.pi/2]
            measuredAngle = self.obtainRotorEulerAngle(obsvCircleCenter[0:3,3],obsvDirection) 

            observ[i]=[obsvCircleCenter[0:3,3],obsvDirection, measuredAngle]
            print(smb.tr2rpy(measuredMatrix[i],'deg'),measuredAngle)
        
        return observ

    def obtainRotorEulerAngle(self,rotateCenter,yawDirection):
        zAxis = yawDirection/np.linalg.norm(yawDirection)
        pitchDirection = np.cross(yawDirection,-rotateCenter)
        yAxis = pitchDirection/np.linalg.norm(pitchDirection)
        rollDirection = np.cross(pitchDirection,yawDirection)
        xAxis = rollDirection/np.linalg.norm(rollDirection)

        mat = np.row_stack([xAxis,yAxis,zAxis])
        hommat = smb.rt2tr(mat,np.zeros((1,3)))
        ea = smb.tr2rpy(hommat,'deg')
        #y, p, r = transformations.euler_from_matrix(mat,axes='rzyx')

        return ea.tolist()

    def extractMeasuringObservations(self,SAInterface, measuredMatrix):
        SAInterface.delete_object('A', 'mainAxis', 'Group')
        # SAInterface.delete_object('A', 'rotorAxis', 'Cylinder')
        SAInterface.delete_object('A', 'rotorAxis', 'Circle')

        lt = list(measuredMatrix.values())
        for i in range(len(lt)):
            SAInterface.construct_point('A','mainAxis','p'+str(i+1),lt[i][0,3],lt[i][1,3],lt[i][2,3])
        SAInterface.fit_geometry('Circle','A','mainAxis','rotorAxis')
        result = SAInterface.get_circle_properties('A','rotorAxis')

        vectorz = np.array(result[1]) if result[1][2]>0 else -np.array(result[1])

        # vectorz=np.array([0.0,0.0,1.0])
        observ = {}
        for i in measuredMatrix.keys(): 
            observMatrix = np.linalg.inv(measuredMatrix[i])
            obsvCircleCenter = observMatrix@smb.transl(result[0][0],result[0][1],result[0][2]) 
            obsvDirection = observMatrix[0:3,0:3]@vectorz

            measuredAngle = self.obtainRotorEulerAngle(obsvCircleCenter[0:3,3],obsvDirection) 

            observ[i]=[obsvCircleCenter[0:3,3],obsvDirection, measuredAngle]
            print(smb.tr2rpy(measuredMatrix[i],'deg'),measuredAngle)
        
        average = np.mean(np.array([i[2][2] for i in observ.values()]))

        return observ

    # def buildHeliCoordinates(self,SAInterface, measuredMatrix):
    #     SAInterface.delete_object('A', 'mainAxis', 'Group')
    #     SAInterface.delete_object('A', 'rotorAxis', 'Circle')

    #     lt = list(measuredMatrix.values())
    #     for i in range(len(lt)):
    #         SAInterface.construct_point('A','mainAxis','p'+str(i+1),lt[i][0,3],lt[i][1,3],lt[i][2,3])
    #     SAInterface.fit_geometry('Circle','A','mainAxis','rotorAxis')
    #     result = SAInterface.get_cicle_properties('A','rotorAxis')

    #     vectorz = np.array(result[1]) if result[1][2]>0 else -np.array(result[1])

    #     # vectorz=np.array([0.0,0.0,1.0])
    #     observ = {}
    #     for i in self.rotorModel.nominalRotorAngle:
    #         vectory = np.cross(vectorz,measuredMatrix[i][0:3,3]-np.array(result[0]))
    #         vectory = vectory / np.linalg.norm(vectory) 
    #         vectorx = np.cross(vectory, vectorz)

    #         initRo = np.column_stack([vectorx,vectory,vectorz]) 
    #         measuredzyx = smb.tr2rpy(smb.rt2tr(np.transpose(initRo),np.zeros([1,3]))@measuredMatrix[i])
    #         # initCo = smb.rt2tr(initRo,np.array(result[0]))

    #         observMatrix = np.linalg.inv(measuredMatrix[i])
    #         obsvCircleCenter = observMatrix@smb.transl(result[0][0],result[0][1],result[0][2]) 
    #         obsvDirection = observMatrix[0:3,0:3]@vectorz
    #         measuredAngle = [math.acos(obsvDirection[1])-math.pi/2,math.acos(obsvDirection[0])-math.pi/2]

    #         observ[i]=[obsvCircleCenter[0:3,3],obsvDirection, measuredAngle, measuredzyx]
        
    #     average = np.mean(np.array([i[2][0] for i in observ.values()]))

    #     return observ

    def simulateMeasuringOtherRotorCycle(self,rotorId):
        theoryName = 'jig_theory'
        SAInterface = SA()
        if(not SAInterface.connect()):
            print("unable to connect SA, please Check application existance")
            return

        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/measure_rotor_pose', 'rotors2/MeasureRotorPose')
        
        measuredMatrix = {}
        for i in range(len(self.rotorModel.nominalRotorAngle)):
            if(i==0):
                request = roslibpy.ServiceRequest({'rotorId':rotorId,'rotorAngle':self.rotorModel.nominalRotorAngle[i],'type':2})
            else:
                request = roslibpy.ServiceRequest({'rotorId':rotorId,'rotorAngle':self.rotorModel.nominalRotorAngle[i],'type':1})

            # print('Calling service...')
            result = service.call(request)
            # print('Service response: {}'.format(result['points']))
            points = result['points']
            res = self.obtainHomoMatrix(SAInterface,i+1,points,theoryName)
            measuredMatrix[self.rotorModel.nominalRotorAngle[i]]=np.array(res[0])

        observation = self.extractVerifyingObservations(SAInterface,measuredMatrix)

        #只有一个位置需要顶到上限位，其余均为自由状态测量
        for i in range(len(self.rotorModel.nominalRotorAngle)):
            if(i==0):
                observation[self.rotorModel.nominalRotorAngle[i]].append(2)
            else:
                observation[self.rotorModel.nominalRotorAngle[i]].append(1)

        SAInterface.disconnect()
        return observation

    def simulateVerifyMeasuringCycle(self,rotorId):
        theoryName = 'jig_theory'
        SAInterface = SA()
        if(not SAInterface.connect()):
            print("unable to connect SA, please Check application existance")
            return

        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/measure_rotor_pose', 'rotors2/MeasureRotorPose')
        
        measuredMatrix = {}
        for i in range(len(self.rotorModel.nominalRotorAngle)):
            request = roslibpy.ServiceRequest({'rotorId':rotorId,'rotorAngle':self.rotorModel.nominalRotorAngle[i],'type':1})

            # print('Calling service...')
            result = service.call(request)
            # print('Service response: {}'.format(result['points']))
            points = result['points']
            res = self.obtainHomoMatrix(SAInterface,i+1,points,theoryName)
            measuredMatrix[self.rotorModel.nominalRotorAngle[i]]=np.array(res[0])

        observation = self.extractVerifyingObservations(SAInterface,measuredMatrix)
        for i in observation.keys():
            observation[i].append(1)

        SAInterface.disconnect()
        return observation

    def Round1MeasuringCycle(self):
        theoryName = 'jig_theory'

        SAInterface = SA()
        if(not SAInterface.connect()):
            print("unable to connect SA, please Check application existance")
            return
        self.InsertTheoryJigPoints(SAInterface,theoryName)

        # rosServer = self.getValidRosServer()
        # service = roslibpy.Service(rosServer, '/measure_rotor_pose', 'rotors2/MeasureRotorPose')
        
        allRoundPoints = self.getPointsFromSA(SAInterface,'rotor11',4)
        measuredMatrix = {}
        for i in range(len(self.rotorModel.nominalRotorAngle)):
            #basic pointing axis is obtained by measuring rotor 1 in type 0 with top limit reached
            # request = roslibpy.ServiceRequest({'rotorId':1,'rotorAngle':self.rotorModel.nominalRotorAngle[i],'type':2})
            # print('Calling service...')
            # result = service.call(request)
            # print('Service response: {}'.format(result['points']))
            # points = result['points']

            points = allRoundPoints[i]
            res = self.obtainHomoMatrix(SAInterface,i+1,points,theoryName)
            measuredMatrix[self.rotorModel.nominalRotorAngle[i]]=np.array(res[0])

        allRoundPoints = self.getPointsFromSA(SAInterface,'rotor12',4)
        for i in range(len(self.rotorModel.additionRotorAngle)):
            points = allRoundPoints[i]
            res = self.obtainHomoMatrix(SAInterface,i+1,points,theoryName)
            measuredMatrix[self.rotorModel.additionRotorAngle[i]]=np.array(res[0])

        observation = self.extractMeasuringObservations(SAInterface,measuredMatrix)

        #添加指标表明此为上限位测量值
        for i in observation.keys():
            observation[i].append(2)

        SAInterface.disconnect()
        return observation

    def controlPanCommand(self,level,alpha,beta):

        rosServer = self.getValidRosServer()
        pub = roslibpy.Topic(rosServer,'pan_command','std_msgs/Float64MultiArray')
        pub.publish({'data':[level,alpha,beta]})
        return

    def simulateMeasuringCycle(self,rotorId):
        theoryName = 'jig_theory'

        SAInterface = SA()
        if(not SAInterface.connect()):
            print("unable to connect SA, please Check application existance")
            return
        self.InsertTheoryJigPoints(SAInterface,theoryName)

        rosServer = self.getValidRosServer()
        service = roslibpy.Service(rosServer, '/measure_rotor_pose', 'rotors2/MeasureRotorPose')
        
        measuredMatrix = {}
        for i in range(len(self.rotorModel.nominalRotorAngle)):
            #basic pointing axis is obtained by measuring rotor 1 in type 0 with top limit reached
            request = roslibpy.ServiceRequest({'rotorId':rotorId,'rotorAngle':self.rotorModel.nominalRotorAngle[i],'type':2})
            # print('Calling service...')
            result = service.call(request)
            # print('Service response: {}'.format(result['points']))
            points = result['points']
            res = self.obtainHomoMatrix(SAInterface,i+1,points,theoryName)
            measuredMatrix[self.rotorModel.nominalRotorAngle[i]]=np.array(res[0])

        observation = self.extractMeasuringObservations(SAInterface,measuredMatrix)

        #添加指标表明此为上限位测量值
        for i in observation.keys():
            observation[i].append(2)

        SAInterface.disconnect()
        return observation


    # def getProcessTree(self):
    #     rpcServer = self.getValidRpcServer()
    #     indexTree = rpcServer.getIndexTree()
    #     return indexTree

    # def getStateTransferIndics(self,index):
    #     rpcServer = self.getValidRpcServer()
    #     indics = rpcServer.getStateTransferIndics(index)
    #     return indics

    # def getActiveModuleIndics(self):
    #     rpcServer = self.getValidRpcServer()
    #     indics = rpcServer.getActiveModuleIndics()
    #     return indics

    # def startTaskExecution(self, index):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.startTaskExecution(index)
    #     return ret

    # def tryCompleteTask(self):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.tryCompleteTask()
    #     return ret

    # def transferModuleState(self, index):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.transferModuleState(index)
    #     return ret

    # def callModuleFunction(self,index,funname,param):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.callModuleFunction(index,funname,param)
    #     return ret

    # def updatePictureModuleMapping(self,mappingTable):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.updatePictureModuleMapping(mappingTable)
    #     return ret

    # def saveAllStateToDatabase(self):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.saveAllStateToDatabase()
    #     return ret

    # def getModuleStateInfo(self,currIndex):
    #     rpcServer = self.getValidRpcServer()
    #     ret = rpcServer.getModuleStateInfo(currIndex)
    #     return ret

class rotorMeasuringScheme:
    def __init__(self,type):
        self.type = type
        self.typeSchemeMapping = {}
        self.typeSchemeMapping['simulating'] = self.wholeSimulationFunction
        self.typeSchemeMapping['firstRoundTesting'] = self.firstRoundTesting
        self.typeSchemeMapping['evaluatingPanEstimation'] = self.evaluatingPanEstimation
    
    def run(self):
        self.typeSchemeMapping[self.type]() 

    def evaluatingPanEstimation(self):
        r = rotorModulingDesktop("192.168.1.4")
        config = nominalRotorConfig(r.rotorModel)

        r.controlPanCommand(0.0,0.0,0.0)
        #建立白色支臂初次测量和主桨榖的方向向量
        result = r.simulateMeasuringCycle(1)
        print("simlating measuring process:", result)

        r.controlPanCommand(30.0,0.0,5.0)
        result = r.simulateVerifyMeasuringCycle(1)
        print("simlating measuring process:",result)

        #更新远程DT模型，专指拉杆长度
        para = r.evaluatePanCommand(result,config.standardPanConfig,1)
        print("dt model is established:",para)

        rodVarying = r.modulingWithDTModel2(config,para,1)
        print("rod length is calculated:", rodVarying)

    def wholeSimulationFunction(self):
        r = rotorModulingDesktop("192.168.220.128")
        config = nominalRotorConfig(r.rotorModel)

        #建立白色支臂在基准位置测量和主桨榖的方向向量
        r.controlPanCommand(config.standardPanConfig[0],config.standardPanConfig[1],config.standardPanConfig[2])
        result = r.simulateMeasuringCycle(1)
        print("simlating measuring process:",result)

        #更新远程DT模型，专指拉杆长度
        para = r.updateDTModel(result,config.standardPanConfig,1)
        print("dt model is established:",para)
        config.updateRotorState(1,para[0],para[2])

        #计算当前拉杆与基准位置两个门限之间的调节量，假设此时尚未调节到位
        rodVarying = r.modulingToRodRange(config,1)
        print("rod length is calculated:", rodVarying)

        #依据结果调节拉杆长度后再次更新DT模型，可以只依据某一个位置的测量值实现更新
        r.controlPanCommand(config.standardPanConfig[0],config.standardPanConfig[1],config.standardPanConfig[2])
        result = r.simulateVerifyMeasuringCycle(1)
        #result = r.simulateMeasuringCycle(1)
        result2={r.rotorModel.nominalRotorAngle[0]:result[r.rotorModel.nominalRotorAngle[0]]}
        para = r.updateDTModel(result2,config.standardPanConfig,1)
        print("dt model is established:",para)

        #计算当前拉杆长度条件下的门限值，如果合理则更新记录
        rodVarying = r.modulingToRodRange(config,1)
        print("rod length is calculated:", rodVarying)
        config.updateRodLengthRange(1,rodVarying)
        
        #其他支臂的测量和调节，只有第一个位置抬起到挥舞极限，其他均可放下测量
        #如果出现超差状况其调节和测量验证方法完全相同，此处假设一次性检验通过
        for i in range(5):
            otherRotorId = i+2
            r.controlPanCommand(config.standardPanConfig[0],config.standardPanConfig[1],config.standardPanConfig[2])

            result = r.simulateMeasuringOtherRotorCycle(otherRotorId)
            #result = r.simulateMeasuringCycle(otherRotorId)

            para = r.updateDTModel(result,config.standardPanConfig,otherRotorId)
            print("dt model is established:",para)
            config.updateRotorState(otherRotorId,para[0],para[2])

            rodVarying = r.modulingToRodRange(config,otherRotorId)
            print("rod length is calculated:", rodVarying)
            config.updateRodLengthRange(otherRotorId,rodVarying)

        #调节总距，周期变距等一系列测量操作步骤，所有测量均可放下，不需要处于挥舞极限位置
        r.controlPanCommand(30.0,5.0,0.0)
        result = r.simulateVerifyMeasuringCycle(1)
        print("simlating measuring process:",result)

        #针对每一组值估算自倾仪状态 , 此处需要进行多次记录 
        para = r.evaluatePanCommand(result,config.standardPanConfig, 1)
        print("dt model is established:",para)
        evalPan = [para[0],para[1],para[2]]

        #计算与理论值的拉杆变动量以及对应的角度测量值
        rodVarying = r.modulingWithDTModel2(config,para,1)
        print("rod length is calculated:", rodVarying)
        #依据超差状况决定要调节的拉杆长度量，不能超出基准位置的允许上限

        #在最后一个停靠位置进行调节
        r.controlPanCommand(30.0,5.0,0.0)
        result = r.simulateVerifyMeasuringCycle(1)

        #调节完毕后依据测量结果更新杆长
        result2={r.rotorModel.nominalRotorAngle[0]:result[r.rotorModel.nominalRotorAngle[0]]}
        para = r.updateDTModel(result2,evalPan,1)
        print("dt model is established:",para)

        #检查满足基准位置的范围
        rodVarying = r.modulingToRodRange(config,1)
        print("rod length is calculated:", rodVarying)
        config.updateRodLengthRange(1,rodVarying)

        #依据多条记录检查满足所有其他位置的角度误差需求
        rodVarying = r.modulingWithDTModel2(config,evalPan,1)
        print("rod length is calculated:", rodVarying)

        r.close()

    def firstRoundTesting(self):
        config = nominalRotorConfig("A903")
        r = rotorModulingDesktop("192.168.6")

        result = r.Round1MeasuringCycle()
        print("simlating measuring process:",result)

        para = r.updateDTModel(result,[0.0,0.0,0.0],1)
        print("dt model is established:",para)

if __name__=='__main__':
    scheme = rotorMeasuringScheme('simulating')
    scheme.run()
